#include "rclcpp/rclcpp.hpp"
#include "message_filters/subscriber.h"
#include "message_filters/synchronizer.h"
#include "message_filters/sync_policies/approximate_time.h"
#include "sensor_msgs/msg/image.hpp"
#include "cv_bridge/cv_bridge.h"
#include <Eigen/Dense>
#include "opencv2/opencv.hpp"
#include "rm_interfaces/msg/shoot_fire_target.hpp"
#include "rm_interfaces/msg/shoot_fire_targets.hpp"
#include <image_transport/image_transport.hpp>
#include <tf2_ros/buffer.h>
#include <tf2_ros/transform_listener.h>
#include <tf2_geometry_msgs/tf2_geometry_msgs.hpp>

using namespace std::chrono_literals;

// 先声明同步策略类型
using SyncPolicy = message_filters::sync_policies::ApproximateTime<
    sensor_msgs::msg::Image,
    rm_interfaces::msg::ShootFireTargets
>;

// 定义节点类，继承自 rclcpp::Node
class DrawImgNode : public rclcpp::Node {
public:
    DrawImgNode() : Node("DrawImgNode_subscriber") {
      camera_matrix = (cv::Mat_<double>(3,3) << 
               1715.30109,    0.     ,  629.87918,
            0     , 1715.21575,  501.54652,
            0     ,    0     ,    1     );
            
      dist_coeffs = (cv::Mat_<double>(5,1)<< -0.044063, 0.115339, -0.000040, 0.000693, 0.000000);

      tf2_buffer_ = std::make_shared<tf2_ros::Buffer>(this->get_clock());
      tf2_listener_ = std::make_shared<tf2_ros::TransformListener>(*tf2_buffer_);

    /* ----------------------------------------- */
    /* 步骤 1: 创建三个订阅者，分别订阅三个话题 */
    /* ----------------------------------------- */
    // 参数说明：
    // - this: 绑定当前节点
    // - "topic1": 话题名称
    sub1_.subscribe(this, "armor_detector/result_img");
    sub2_.subscribe(this, "armor_solver/ShootFireTargets");

    result_img_pub_ = image_transport::create_publisher(this, "draw_img/result_result_img");

    /* ----------------------------------------- */
    /* 步骤 2: 定义同步策略（处理三个消息类型） */
    /* ----------------------------------------- */
    // ApproximateTime 策略允许消息时间戳存在一定误差
    // 模板参数为三个订阅话题的消息类型（此处均为 std_msgs::msg::String）
    using SyncPolicy = message_filters::sync_policies::ApproximateTime<
        sensor_msgs::msg::Image,
        rm_interfaces::msg::ShootFireTargets
    >;

    /* ----------------------------------------- */
    /* 步骤 3: 初始化同步器并配置参数 */
    /* ----------------------------------------- */
    // 参数说明：
    // - SyncPolicy(10): 队列大小为10（缓存未匹配消息的数量）
    // - sub1_, sub2_, sub3_: 绑定的订阅者
    sync_ = std::make_shared<message_filters::Synchronizer<SyncPolicy>>(
      SyncPolicy(10), 
      sub1_, 
      sub2_
    );
    
    // 设置最大允许时间差为 200ms（消息时间戳差异超过此值则不触发回调）
    sync_->setMaxIntervalDuration(rclcpp::Duration(200ms)); 

    // 注册同步回调函数，this 表示将当前对象指针传递给回调
    sync_->registerCallback(&DrawImgNode::sync_callback, this);
  }

private:
  /* ----------------------------------------- */
  /* 步骤 4: 定义同步回调函数 */
  /* ----------------------------------------- */
  // 参数说明：
  // - msg1, msg2, msg3: 三个话题的消息指针（按订阅者顺序排列）
  void sync_callback(
    const sensor_msgs::msg::Image::ConstSharedPtr &img_msg,
    const rm_interfaces::msg::ShootFireTargets::SharedPtr &SFTs
  ) {
    cv::Mat img;
    img = cv_bridge::toCvShare(img_msg, "rgb8")->image;
    for(auto &STF : SFTs->targets){
     
      geometry_msgs::msg::PoseStamped ps;
      ps.header = SFTs->header;
      ps.pose = STF.pose;
      try{
        STF.pose = tf2_buffer_->transform(ps,"camera_optical_frame").pose;
      }
      catch (const tf2::TransformException &ex) {
        RCLCPP_ERROR(this->get_logger(), "Transform error: %s", ex.what());
        return;
      }
    }

    std::vector<Eigen::Vector3d> firetargets(4);  // 提前分配 4 个元素
    std::vector<Eigen::Vector3d> shoottargets(4); // 提前分配 4 个元素
    Eigen::Vector3d final_shoottarget;
    Eigen::Vector3d final_firetarget;
    if(SFTs->targets.size()>5)
    {

      for(int i = 0;i<4;i++)
      {
  
        firetargets[i].x() = SFTs->targets[i].pose.position.x;
        firetargets[i].y() = SFTs->targets[i].pose.position.y;
        firetargets[i].z() = SFTs->targets[i].pose.position.z;
  
      }
      for(int i = 4;i<8;i++)
      {
     
        shoottargets[i-4].x() = SFTs->targets[i].pose.position.x;
        shoottargets[i-4].y() = SFTs->targets[i].pose.position.y;
        shoottargets[i-4].z() = SFTs->targets[i].pose.position.z;
 
      }
    }
    final_shoottarget = shoottargets[SFTs->index];
    final_firetarget = firetargets[SFTs->index];

    cv::Point3f shoot_target_3d(final_shoottarget.x(),final_shoottarget.y(),final_shoottarget.z());
    cv::Point3f fire_target_3d(final_firetarget.x(),final_firetarget.y(),final_firetarget.z());

    std::vector<cv::Point3f>point_3d = {shoot_target_3d,fire_target_3d};
    std::vector<cv::Point2f>point_2d;

    cv::projectPoints(point_3d,
                      cv::Mat::zeros(3,1,CV_64F),
                      cv::Mat::zeros(3,1,CV_64F),
                      camera_matrix,
                      dist_coeffs,
                      point_2d);
    
    for(size_t i = 0;i<point_2d.size();i++)
    {
      int size = 10;  // 正方形大小
      cv::Point2f pt = point_2d[i];
      // 检查点是否在图像范围内
      if(pt.x >= 0 && pt.x < img.cols && pt.y >= 0 && pt.y < img.rows) {
          // 绘制正方形
          cv::rectangle(img, 
                       cv::Point(pt.x - size/2, pt.y - size/2),
                       cv::Point(pt.x + size/2, pt.y + size/2),
                       i == 0 ? cv::Scalar(0, 255, 0) : cv::Scalar(0, 0, 255), 2); // 第一个点绿
    }
    std::cout<<"成功发布"<<std::endl;
    result_img_pub_.publish(cv_bridge::CvImage(img_msg->header, "rgb8", img).toImageMsg());
  }
  };
  // 成员变量声明
  message_filters::Subscriber<sensor_msgs::msg::Image> sub1_; // 话题1订阅者
  message_filters::Subscriber<rm_interfaces::msg::ShootFireTargets> sub2_; // 话题2订阅者
  std::shared_ptr<message_filters::Synchronizer<SyncPolicy>> sync_; // 同步器指针

  image_transport::Publisher result_img_pub_;//图像的发布者
  std::shared_ptr<tf2_ros::Buffer> tf2_buffer_;
  std::shared_ptr<tf2_ros::TransformListener> tf2_listener_;

  cv::Mat  camera_matrix;
  cv::Mat dist_coeffs;

};


/* ----------------------------------------- */
/* 主函数 */
/* ----------------------------------------- */
int main(int argc, char **argv) {
  rclcpp::init(argc, argv);                              // 初始化 ROS2 上下文
  auto node = std::make_shared<DrawImgNode>();  // 创建节点对象
  rclcpp::spin(node);                                    // 进入事件循环
  rclcpp::shutdown();                                    // 关闭 ROS2 上下文
  return 0;
};